/*****************************************************************************
 * @file     BR_PathFollowing.c
 * @author   FSF
 * @date     2019/12/31
 * @brief    路径跟随
 ******************************************************************************
 * @note     路径跟随算法，在主控中被高频调用
 ******************************************************************************/
#include "BR_Common.h"
#include "arm_math.h"

/* 静态变量 */
static int prvCurrentPathIndex = 0;          // 标志当前目标点

/* 用于处理第一次进入逻辑时 D项 LastErr=0 */
static uint8_t prvVelCalculateEnterFlag;     // 标志第一次进入正常轨迹速度计算逻辑
static uint8_t prvFinalPointXYCorrectFlag;   // 标志第一次进入最终点xy校正逻辑
static uint8_t prvFinalPointThetaCorrectFlag;// 标志第一次进入最终点theta校正逻辑

static uint8_t prvOnFinalPoint = FALSE;      // 最终点找到定位点的指示位

static uint8_t prvSafeCheck(float *pCurTarPoint, float CurrentX, float CurrentY);
static void prvVelCalculate(        BR_Path_t pPathStru, float *Vel, float *Dir, float *Omega, float CurrentX, float CurrentY, float CurrentTheta, \
                                    BR_MotionMode_t *MotionMode);
static void prvFinPointCalculate(   BR_Path_t pPathStru, float *Vel, float *Dir, float *Omega, float CurrentX, float CurrentY, float CurrentTheta, \
                                    BR_MotionMode_t *MotionMode, float *WheelsVel, float *WheelsDir);

/* -------------------------------------------------- Public ----------------------------------------------- */
void BR_vFollowPath(BR_Path_t pPathStru, float *Vel, float *Dir, float *Omega, BR_MotionMode_t *MotionMode, float *WheelsVel, float *WheelsDir)
{
    float (*pPath)[5] = pPathStru.pPath;
    float CurrentX = BR_gRobot.ChassisState.X;
    float CurrentY = BR_gRobot.ChassisState.Y;
    float CurrentTheta = BR_gRobot.ChassisState.Theta;

    /* 安全检查 */
    if(prvSafeCheck(pPath[prvCurrentPathIndex], CurrentX, CurrentY))    // 若偏离目标
    {
        BR_gRobot.StateFlags.eWorkingFlow = IDLE;
        BR_vResetPathFollowing();
        return;
    }
    /* 判断当前位置, 根据情况设置控制模式和速度 */
    if (prvCurrentPathIndex == 0)                                                           // 起始0速点
        prvCurrentPathIndex++;
    else if (pPath[prvCurrentPathIndex][0] != 0 || pPath[prvCurrentPathIndex][1] != 0)      // 大多数情况
    {
        /* 判断是否改变目标点 */
        float TargetX = pPath[prvCurrentPathIndex][2];
        float TargetY = pPath[prvCurrentPathIndex][3];
        float NextTargetX = pPath[prvCurrentPathIndex + 1][2];
        float NextTargetY = pPath[prvCurrentPathIndex + 1][3];
        float DotProduct = ( TargetX - CurrentX ) * ( NextTargetX - TargetX ) +
                           ( TargetY - CurrentY ) * ( NextTargetY - TargetY );
        if (DotProduct < 0)
            prvCurrentPathIndex += 1;
        /* 正常情况下的速度计算和设置逻辑 */
        prvVelCalculate(pPathStru, Vel, Dir, Omega, CurrentX, CurrentY, CurrentTheta, MotionMode);
    }
    else                                                                                    // 最终0速点
        prvFinPointCalculate(pPathStru, Vel, Dir, Omega, CurrentX, CurrentY, CurrentTheta, MotionMode, WheelsVel, WheelsDir);    
}

/** @brief  重置路径跟随至可以开始执行一次跟随的起始状态 */
void BR_vResetPathFollowing(void)
{
    prvCurrentPathIndex = 0;
    prvVelCalculateEnterFlag = 0;
    prvFinalPointXYCorrectFlag = 0;
    prvFinalPointThetaCorrectFlag = 0;
    prvFinalPointXYCorrectFlag = 0;
    prvFinalPointThetaCorrectFlag = 0;
    prvOnFinalPoint = FALSE;
}

/* -------------------------------------------------- Private ----------------------------------------------- */
/** @brief  检查是否偏离当前路径
 *  @retval 0正常, 1偏离
 **/
static uint8_t prvSafeCheck(float *pCurTarPoint, float CurrentX, float CurrentY)
{
    float SafeDistance;
    if (pCurTarPoint[0] == 0 && pCurTarPoint[1] == 0)           // 最终0速点增加一定安全距离
        SafeDistance = BR_configFOLLOWING_SAFE_DISTANCE * 4;
    else
        SafeDistance = BR_configFOLLOWING_SAFE_DISTANCE;
    if (ABS(pCurTarPoint[2] - CurrentX)  +  ABS(pCurTarPoint[3] - CurrentY)  > SafeDistance)   // 若离目标点太远
        return 1;
    return 0;
}

/** @brief  一般情况的路径跟随逻辑 */
static void prvVelCalculate(BR_Path_t pPathStru, float *Vel, float *Dir, float *Omega, float CurrentX, float CurrentY, float CurrentTheta, BR_MotionMode_t *MotionMode)
{
    float NormalVelX, NormalVelY;
    float (*pPath)[5] = pPathStru.pPath;
    static float LastErrTheta;
    static float LastVector4[2];
    /* 主方向分速度计算 */
    float MainVelX = pPath[prvCurrentPathIndex][0];
    float MainVelY = pPath[prvCurrentPathIndex][1];
    /* 法向修正分速度计算 */
    float Vector1[2] = {pPath[prvCurrentPathIndex][2] - CurrentX,                           // 当前到目标
                        pPath[prvCurrentPathIndex][3] - CurrentY};
    float Vector2[2] = {pPath[prvCurrentPathIndex - 1][2] - pPath[prvCurrentPathIndex][2],  // 目标点到前一目标点
                        pPath[prvCurrentPathIndex - 1][3] - pPath[prvCurrentPathIndex][3]};
    float Distance = sqrt(pow(Vector2[0], 2) + pow(Vector2[1], 2));                         // 两目标点间距离
    float Shadow = ABS(( Vector1[0] * Vector2[0] + Vector1[1] * Vector2[1] ) / Distance);   // 投影长度
    float Vector3[2] = {Vector2[0] / Distance * Shadow, Vector2[1] / Distance * Shadow};    // 目标点到前一目标向量的一部分
    float MainVel = sqrt(pow(MainVelX, 2) + pow(MainVelY, 2));
    float NormalP = pPathStru.NormalP * MainVel;                                            // 法向修正的P和当前主速度相关
    float NormalD = pPathStru.NormalD * MainVel;                                            // 法向修正的D和当前主速度相关
    float Vector4[2] = {Vector1[0] + Vector3[0], Vector1[1] + Vector3[1]};                  // 法向修正向量
    if(prvVelCalculateEnterFlag)                                                                // PD调节
    {
        NormalVelX = NormalP * Vector4[0] + NormalD * ( Vector4[0] - LastVector4[0] );
        NormalVelY = NormalP * Vector4[1] + NormalD * ( Vector4[1] - LastVector4[1] );          
    }
    else                                                                                        // 第一次进入逻辑时 P调节
    {
        NormalVelX = NormalP * Vector4[0];
        NormalVelY = NormalP * Vector4[1];                                                      
    }
    LastVector4[0] = Vector4[0];
    LastVector4[1] = Vector4[1];
    /* 自转角速度计算 */
    float ErrTheta = pPath[prvCurrentPathIndex][4] - CurrentTheta;
    if(prvVelCalculateEnterFlag)                                                                // PD调节        
    {
        *Omega = pPathStru.ThetaP * ErrTheta + pPathStru.ThetaD * ( ErrTheta - LastErrTheta );  
    }
    else                                                                                        // 第一次进入逻辑时 P调节
    {
        *Omega = pPathStru.ThetaP * ErrTheta;                                                   
        prvVelCalculateEnterFlag = 1;
    }
    LastErrTheta = ErrTheta;
    /* 速度合成 */
    float VelX = MainVelX + NormalVelX;
    float VelY = MainVelY + NormalVelY;
    *Vel = sqrt(pow(VelX, 2) + pow(VelY, 2));
    *Dir = -atan2(VelX, VelY); 
    *MotionMode = CHASSIS_MODE;
}

/** @brief  最终点的精确定位逻辑 */
static void prvFinPointCalculate(BR_Path_t pPathStru, float *Vel, float *Dir, float *Omega, float CurrentX, float CurrentY, float CurrentTheta, \
                                 BR_MotionMode_t *MotionMode, float *WheelsVel, float *WheelsDir)
{
    float (*pPath)[5] = pPathStru.pPath;
    if(prvOnFinalPoint == 0)                        // 先将xy调节至最终点
    {
        static float LastErr;
        float ErrX = pPath[prvCurrentPathIndex][2] - CurrentX;
        float ErrY = pPath[prvCurrentPathIndex][3] - CurrentY;
        float Err = sqrt(pow(ErrX, 2) + pow(ErrY, 2));        
        /* 判断theta是否到位 */
        if(ABS(Err) < pPathStru.ArriveDistance)
        {        
            prvOnFinalPoint = 1;
            return;
        }
        /* 若未到位 */
        if(prvFinalPointXYCorrectFlag)  // 正常PD调节
        {
            *Vel = BR_configFINAL_POINT_P * Err + BR_configFINAL_POINT_D * ( Err - LastErr );
        }
        else                            // 第一次进入逻辑 P调节
        {
            *Vel = BR_configFINAL_POINT_P * Err + BR_configFINAL_POINT_D;
            prvFinalPointXYCorrectFlag = 1;
        }
        LastErr = Err;
        *Vel = *Vel > BR_configFINAL_POINT_MAX_VEL ? BR_configFINAL_POINT_MAX_VEL : *Vel; // 速度限制
        *Dir = -atan2(ErrX, ErrY);
        *Omega = 0;        
        *MotionMode = CHASSIS_MODE;
        
    }
    else                                            // 再在不转动轮子的情况下调整theta
    {
        float WheelDiffVel;
        static float LastErrTheta;
        static float CosWheelTangentialAngle[4];
        const float foo = atan(BR_configCHASSIS_Y/BR_configCHASSIS_X);  // 适配长方形车体，foo为与车体尺寸相关的常量
        const float TangentialAngle[4] = {foo, -foo, PI-foo, -PI+foo};  // 存自转速度舵轮的转角
        float ErrTheta = pPath[prvCurrentPathIndex][4] - CurrentTheta;
        /* 判断theta是否到位 */
        if (ABS(ErrTheta) < pPathStru.ArriveAngle)  // xy和theta都完成了精确定位，且舵向保持在差速调节方向的状态不动，认为其不会对xy产生影响
        {
            for (int i = 0; i < 4; i++) WheelsVel[i] = 0;
            BR_gRobot.StateFlags.eWorkingFlow = IDLE;
            BR_vResetPathFollowing();
            /* 将自转后舵轮在世界坐标系下的Dir更新，确保回到IDLE状态CHASSIS_MODE模式下舵向保持不动 */
            float tmpDir = BR_gRobot.ServoMotors.RelativePos[0] + BR_gRobot.ChassisState.Theta;
            *Dir = ( tmpDir <= -PI ) ? ( 2 * PI + tmpDir ) : tmpDir;
            *Dir = ( tmpDir >= PI ) ? ( -2 * PI + tmpDir ) : tmpDir;
            *MotionMode = CHASSIS_MODE;
            return;
        }
        /* 若未到位 */
        if(prvFinalPointThetaCorrectFlag)   // 正常PD调节
        {
            WheelDiffVel = BR_configFINAL_DIFF_P * ErrTheta + BR_configFINAL_DIFF_D * ( ErrTheta - LastErrTheta );
        }
        else                                // 第一次进入逻辑 P调节
        {
            WheelDiffVel = BR_configFINAL_DIFF_P * ErrTheta;
            prvFinalPointThetaCorrectFlag = 1;
        }
        WheelDiffVel = WheelDiffVel > BR_configFINAL_DIFF_MAX_VEL ? BR_configFINAL_DIFF_MAX_VEL : WheelDiffVel;
        LastErrTheta = ErrTheta;
        /* 在保持舵向不变的情况下差速转动至目标角度 */
        for (int i = 0; i < 4; i++)
        {
            /* 四个舵轮分别计算实际舵向和自转分量方向（切向）的Delta的cos值*/
            CosWheelTangentialAngle[i] = arm_cos_f32(TangentialAngle[i] - BR_gRobot.ServoMotors.RelativePos[i]);
            /* 判断当前舵向能否提供差速转向的分速度 */
            if(ABS(CosWheelTangentialAngle[i]) > 0.5f)
                WheelsVel[i] = WheelDiffVel / CosWheelTangentialAngle[i];
            else
                WheelsVel[i] = 0;
            WheelsDir[i] = BR_gRobot.ServoMotors.RelativePos[i];
        }         
        *MotionMode = WHEELS_MODE;        
    }
}
